/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, LABUST, UNIZG-FER
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the LABUST nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Author : Dula Nad
 *  Created: 01.02.2013.
 *********************************************************************/
#include "BackwardCompatibility.hpp"
#include "../ConfigXMLParser.h"
#include "../ROSInterface.h"

#include <osg/MatrixTransform>
#include <osg/PositionAttitudeTransform>

//Introduce this dummy to also implement sensor less ROS publisher
class dummy_sensor{};

PP_LABUST_MAKE_SENSORFACTORY(USBLSensorFactory,	 //The factory name
		USBLSensor, //The sensor class
		XMLUSBLSensor, //The XML configuration class of the sensor
		USBLSensorToROS, //The ROSInterface class of the sensor
		&ConfigFile::processUSBLSensor, //The already existing xml parser function
		"usblSensorGen", //The XML sensor name
		"USBLSensorGenToROS", //The ROS XML config name
		(hook->scene)(config->name)(hook->vMs) //Stuff to be passed to the sensor constructor during creation
		(hook->scene->localizedWorld->getMatrix())
		(config->std)(config->transponders))

PP_LABUST_MAKE_SENSORFACTORY(GPSSensorFactory,
		GPSSensor,
		XMLGPSSensor,
		GPSSensorToROS,
		&ConfigFile::processGPSSensor,
		"gpsSensorGen",
		"GPSSensorGenToROS",
		(hook->scene)(config->name)(hook->vMs)
		(hook->scene->localizedWorld->getMatrix())
		(config->std))

PP_LABUST_MAKE_SENSORFACTORY(DVLSensorFactory,
		DVLSensor,
		XMLDVLSensor,
		DVLSensorToROS,
		&ConfigFile::processDVLSensor,
		"dvlSensorGen",
		"DVLSensorGenToROS",
		(config->name)(hook->vMs)
		(hook->scene->localizedWorld->getMatrix())
		(config->std))

PP_LABUST_MAKE_SENSORFACTORY(PressureSensorFactory,
		PressureSensor,
		XMLPressureSensor,
		PressureSensorToROS,
		&ConfigFile::processPressureSensor,
		"pressureSensorGen",
		"PressureSensorGenToROS",
		(config->name)(hook->vMs)
		(hook->scene->localizedWorld->getMatrix())
		(config->std))

PP_LABUST_MAKE_SENSORFACTORY(ImuSensorFactory,
		InertialMeasurementUnit,
		Imu,
		ImuToROSImu,
		&ConfigFile::processImu,
		"imuGen",
		"ImuGenToROSImu",
		(config->name)(hook->vMs)
		(hook->scene->localizedWorld->getMatrix())
		(config->std))

PP_LABUST_MAKE_SENSORFACTORY(RangeSensorFactory,
		VirtualRangeSensor,
		rangeSensor,
		RangeSensorToROSRange,
		&ConfigFile::processRangeSensor,
		"rangeSensorGen",
		"RangeSensorGenToROSRange",
		(config->name)(hook->scene->localizedWorld)
		(hook->vMs)
		(config->range)
		(config->visible? true:false))

class RangeSensorFactoryDetail : public virtual RangeSensorFactory
{
public:
	RangeSensorFactoryDetail():Base(&ConfigFile::processRangeSensor,"rangeSensorGen",
			"RangeSensorGenToROSRange"){};

	void createSensor(plugin::UWSimHook::Ptr hook)
	{
		hook->urdf->link[config->link]->asGroup()->addChild(hook->vMs);
		sensor.reset(new VirtualRangeSensor(config->name, hook->scene->localizedWorld, hook->vMs, config->range, config->visible? true:false));
	}
};

//Abuse the sensor factory interface for sensor-less ROS interfaces - notice that XML sensor name should be equal to ROS XML name
PP_LABUST_MAKE_ROSINTERFACEFACTORY(OdomToPATSensorFactory,
		ROSOdomToPAT,
		"ROSOdomToPATGen",
		(hook->root)
		(rosInterface.topic)
		(rosInterface.targetName)
		(rosInterface.color)
		(rosInterface.visualize))

extern "C"
{
//The first draft of the actual sensor collection exported function in a plugin.
void registerSensors(plugin::SensorList& list, plugin::ROSInterfaceList& rlist)
{
	std::cout<<"Adding plugins."<<std::endl;

	list.push_back(plugin::SensorInterface::Ptr(new USBLSensorFactory()));
	list.push_back(plugin::SensorInterface::Ptr(new GPSSensorFactory()));
	list.push_back(plugin::SensorInterface::Ptr(new DVLSensorFactory()));
	list.push_back(plugin::SensorInterface::Ptr(new ImuSensorFactory()));
	list.push_back(plugin::SensorInterface::Ptr(new RangeSensorFactoryDetail()));
	list.push_back(plugin::SensorInterface::Ptr(new PressureSensorFactory()));
}
};
